#include "odrive_can.h"

void set_position(CAN_HandleTypeDef *_hcan, uint8_t odrive_axis_can_id, float Input_Pos, int Vel_FF, int Torque_FF) {
    odrive_axis_can_id = (odrive_axis_can_id << 5) | SET_INPUT_POS;

    uint8_t *ptrPos;
    ptrPos = (uint8_t *) &Input_Pos;
    uint8_t *ptrVel;
    ptrVel = (uint8_t *) &Vel_FF;
    uint8_t *ptrTor;
    ptrTor = (uint8_t *) &Torque_FF;

    uint8_t data[8] = {ptrPos[0], ptrPos[1], ptrPos[2], ptrPos[3], ptrVel[0], ptrVel[1], ptrTor[0], ptrTor[1]};
    o_can_send_message_Data(_hcan, odrive_axis_can_id, data, 8);
}

void set_vel(CAN_HandleTypeDef *_hcan, uint8_t odrive_axis_can_id, float Vel, float Tor_FF) {
    odrive_axis_can_id = (odrive_axis_can_id << 5) | SET_INPUT_VEL;

    uint8_t *ptrVel;
    ptrVel = (uint8_t *) &Vel;
    uint8_t *ptrTor;
    ptrTor = (uint8_t *) &Tor_FF;

    uint8_t data[8] = {ptrVel[0], ptrVel[1], ptrVel[2], ptrVel[3], ptrTor[0], ptrTor[1], ptrTor[2], ptrTor[3]};

    o_can_send_message_Data(_hcan, odrive_axis_can_id, data, 8);
}

void set_torque(CAN_HandleTypeDef *_hcan, uint8_t odrive_axis_can_id, float torque) {
    odrive_axis_can_id = (odrive_axis_can_id << 5) | SET_INPUT_TORQUE;
    uint8_t *ptrTor;
    ptrTor = (uint8_t *) &torque;
    uint8_t data[4] = {ptrTor[0], ptrTor[1], ptrTor[2], ptrTor[3]};
    o_can_send_message_Data(_hcan, odrive_axis_can_id, data, 4);
}

void Set_Limits(CAN_HandleTypeDef *_hcan, uint8_t odrive_axis_can_id, float vel_lim, float curr_lim) {
    odrive_axis_can_id = (odrive_axis_can_id << 5) | SET_LIMITS;
    uint8_t *ptrVelLim;
    ptrVelLim = (uint8_t *) &vel_lim;
    uint8_t *ptrCurrLim;
    ptrCurrLim = (uint8_t *) &curr_lim;
    uint8_t data[8] = {ptrVelLim[0], ptrVelLim[1], ptrVelLim[2], ptrVelLim[3], ptrCurrLim[0], ptrCurrLim[1],
                       ptrCurrLim[2], ptrCurrLim[3]};
    o_can_send_message_Data(_hcan, odrive_axis_can_id, data, 8);
}

void Set_Position_Gain(CAN_HandleTypeDef *_hcan, uint8_t odrive_axis_can_id, float pos_gain) {
    odrive_axis_can_id = (odrive_axis_can_id << 5) | SET_POSITION_GAIN;
    uint8_t *ptrPos;
    ptrPos = (uint8_t *) &pos_gain;
    uint8_t data[4] = {ptrPos[0], ptrPos[1], ptrPos[2], ptrPos[3]};
    o_can_send_message_Data(_hcan, odrive_axis_can_id, data, 4);
}

void Set_Vel_Gains(CAN_HandleTypeDef *_hcan, uint8_t odrive_axis_can_id, float Vel_Gain, float Vel_Int_Gain) {
    odrive_axis_can_id = (odrive_axis_can_id << 5) | SET_VEL_GAINS;
    uint8_t *ptrVelGain;
    ptrVelGain = (uint8_t *) &Vel_Gain;
    uint8_t *ptrVelIntGain;
    ptrVelIntGain = (uint8_t *) &Vel_Int_Gain;
    uint8_t data[8] = {ptrVelGain[0], ptrVelGain[1], ptrVelGain[2], ptrVelGain[3], ptrVelIntGain[0], ptrVelIntGain[1],
                       ptrVelIntGain[2], ptrVelIntGain[3]};
    o_can_send_message_Data(_hcan, odrive_axis_can_id, data, 8);
}

void Set_Axis_Requested_State(CAN_HandleTypeDef *_hcan, uint8_t odrive_axis_can_id, Axis_State state) {
    odrive_axis_can_id = (odrive_axis_can_id << 5) | SET_AXIS_REQUESTED_STATE;
    unsigned int Requested_State = state;
    uint8_t *ptrToFloat;
    ptrToFloat = (uint8_t *) &Requested_State;
    uint8_t data[4] = {ptrToFloat[0], ptrToFloat[1], ptrToFloat[2], ptrToFloat[3]};
    o_can_send_message_Data(_hcan, odrive_axis_can_id, data, 4);
}

void Set_Controller_Modes(CAN_HandleTypeDef *_hcan, uint8_t odrive_axis_can_id, Control_Mode ControlMode, Input_Mode InputMode) {
    odrive_axis_can_id = (odrive_axis_can_id << 5) | SET_CONTROLLER_MODES;
    int Control = ControlMode;
    int Input = InputMode;
    uint8_t *ptrControl;
    ptrControl = (uint8_t *) &Control;
    uint8_t *ptrInput;
    ptrInput = (uint8_t *) &Input;
    uint8_t data[8] = {ptrControl[0], ptrControl[1], ptrControl[2], ptrControl[3], ptrInput[0], ptrInput[1],
                       ptrInput[2], ptrInput[3]};
    o_can_send_message_Data(_hcan, odrive_axis_can_id, data, 8);
}

void Set_Axis_Node_ID(CAN_HandleTypeDef *_hcan, uint8_t odrive_axis_can_id, uint32_t node_id) {
    odrive_axis_can_id = (odrive_axis_can_id << 5) | SET_AXIS_NODE_ID;
    uint8_t *ptrNodeId;
    ptrNodeId = (uint8_t *) &node_id;
    uint8_t data[4] = {ptrNodeId[0], ptrNodeId[1], ptrNodeId[2], ptrNodeId[3]};
    o_can_send_message_Data(_hcan, odrive_axis_can_id, data, 4);
}

void Get_Bus_Voltage_Current(CAN_HandleTypeDef *_hcan, uint8_t odrive_axis_can_id) {
    odrive_axis_can_id = (odrive_axis_can_id << 5) | GET_BUS_VOLTAGE_CURRENT;
    o_can_send_message_Remoto(_hcan, odrive_axis_can_id, NULL, 0);
}

void Get_IQ(CAN_HandleTypeDef *_hcan, uint8_t odrive_axis_can_id) {
    odrive_axis_can_id = (odrive_axis_can_id << 5) | GET_IQ;
    o_can_send_message_Remoto(_hcan, odrive_axis_can_id, NULL, 0);
}

void Get_Encoder_Count(CAN_HandleTypeDef *_hcan,uint8_t odrive_axis_can_id) {
    odrive_axis_can_id = (odrive_axis_can_id << 5) | GET_ENCODER_COUNT;
    o_can_send_message_Remoto(_hcan, odrive_axis_can_id, NULL, 0);
}

void Odrive_ESTOP(CAN_HandleTypeDef *_hcan, uint8_t odrive_axis_can_id) {
    odrive_axis_can_id = (odrive_axis_can_id << 5) | ODRIVE_ESTOP_MESSAGE;
    o_can_send_message_Data(_hcan, odrive_axis_can_id, NULL, 0);
}

void Start_Anticogging(CAN_HandleTypeDef *_hcan, uint8_t odrive_axis_can_id) {
    odrive_axis_can_id = (odrive_axis_can_id << 5) | START_ANTICOGGING;
    o_can_send_message_Data(_hcan, odrive_axis_can_id, NULL, 0);
}

void reboot_odrive(CAN_HandleTypeDef *_hcan, uint8_t odrive_axis_can_id) {
    odrive_axis_can_id = (odrive_axis_can_id << 5) | REBOOT_ODRIVE;
    o_can_send_message_Data(_hcan, odrive_axis_can_id, NULL, 0);
}

void Clear_Errors(CAN_HandleTypeDef *_hcan, uint8_t odrive_axis_can_id) {
    odrive_axis_can_id = (odrive_axis_can_id << 5) | CLEAR_ERRORS;
    o_can_send_message_Data(_hcan, odrive_axis_can_id, NULL, 0);
}

void o_can_send_message_Data(CAN_HandleTypeDef *_hcan, uint32_t id, uint8_t *data, uint8_t len) {
    CAN_Send_Msg(_hcan, id, data, len, CAN_ID_STD, CAN_RTR_DATA);
}

void o_can_send_message_Remoto(CAN_HandleTypeDef *_hcan, uint32_t id, uint8_t *data, uint8_t len) {
    CAN_Send_Msg(_hcan, id, data, len, CAN_ID_STD, CAN_RTR_REMOTE);
}